双臂标定功能开发介绍

1. 功能描述

基于双臂标定工装，对一对机械臂的 dh 参数进行标定，输出 dh 参数估计值

算法支持的版本：aral-0.38.1+5cfd9e3-Linux_x86_64.zip 及以上

2. 接口介绍

接口链接：接口

    /**
     * @brief 标定机械臂运动学参数（dh参数）
     * @param nom_para: 运动学模型理论值
     * @param data_input: 输入的标定数据
     * @param est_para: 运动学模型估计值
     * @param res_error: 估计值误差, 分别为［标定数据位置误差，姿态误差，校验数据位置误差，姿态误差］
     * @return　返回值 < 0, 表示计算失败
     */
    ARAL_API_COMMON(1.0) int calibRobotKinematicParameterBaseDual(const RobotKinCalibPara& nom_para, const std::vector<RobotKinCalibDualInput>& data_input,
                                                          RobotKinCalibPara& est_para, std::vector<DataFeature>& res_error) = 0;

3. 调用例程

测试链接：见压缩包里面的测试文件 test/src/calib_module/test_calib_robot_dh_para.cpp

SUITE(SUITE_ARAL_CALIB_ROBOT_DH_PARA)
{
    //! 测试接口calibRobotKinematicParameterBaseDual
    TEST_FIXTURE(AuboRobotInterface, testCalibRobotKinematicParameterBaseDual)
    {
        Setup("aubo_i5");

        //! 1.标定参数设置
        interface::RobotKinCalibPara nom_para;
        nom_para.dh1 = robot->mdlGetRobotDHParameter(false);
        nom_para.dh2 = robot->mdlGetRobotDHParameter(false);
        nom_para.base = {-1.57079633 ,-0.10000000 ,-2.09439510 ,-0.40000000 ,-1.57079633 , 0.10000000};
        nom_para.tcp = {3.14159265,  0.00950000, -1.57079633, -0.23000000,  0.00000000, -0.00950000};
        nom_para.choose_vec = {0, 0, 1, 1, 0, 0, // base rz tz rx tx rz tz
                               0, 0, 1, 1, 0,    // robot1 link1 alpha a d theta beta
                               1, 1, 1, 1, 0,
                               1, 1, 0, 1, 1,
                               1, 1, 0, 1, 1,
                               1, 1, 1, 1, 0,
                               1, 1, 1, 1, 0,
                               0, 0, 1, 0, 0, 0, // tcp rz tz rx tx rz tz
                               0, 0, 1, 1, 0,    // robot2 link1 alpha a d theta beta
                               1, 1, 1, 1, 0,
                               1, 1, 0, 1, 1,
                               1, 1, 0, 1, 1,
                               1, 1, 1, 1, 0,
                               1, 1, 1, 1, 0};


        //! 2.读取采集数据
        std::string recordFileName = "./test/data/calibration/robot_dh_calib/testdata.csv";
        std::vector<std::vector<double>> data_record;
        int ret = ReadFromFile2D(recordFileName, ",", data_record);
        CHECK(ret >= 0);

        std::vector<interface::RobotKinCalibDualInput> data_input;
        data_input.resize(data_record.size());
        for(unsigned int i = 0; i < data_input.size(); i++)
        {
            for(unsigned int j = 0; j < data_input[0].q1.size(); j++)
            {
                data_input[i].q1[j]    = data_record[i][j];
                data_input[i].tau1[j]  = data_record[i][j + dof];
                data_input[i].q2[j]    = data_record[i][j + 2 * dof];
                data_input[i].tau2[j]  = data_record[i][j + 3 * dof];
            }
        }

        //! 3.标定
        interface::RobotKinCalibPara est_para;
        std::vector<interface::DataFeature> res_error;
        ret = robot->calibRobotKinematicParameterBaseDual(nom_para, data_input, est_para, res_error);

        //! 4.结果校验
        CHECK(ret >= 0);
        if(ret >= 0)
        {
            std::string recordFileNameRef = "./test/data/calibration/robot_dh_calib/res_ref.txt";
            std::vector<double> res_ref;
            ret = ReadFromFile1D(recordFileNameRef, res_ref);
            CHECK(ret >= 0);

            std::vector<double> res_out;
            for(unsigned int i = 0; i < est_para.base.size(); i++)
            {
                res_out.push_back(est_para.base[i]);
            }
            for(unsigned int i = 0; i < dof; i++)
            {
                res_out.push_back(est_para.dh1.alpha[i]);
                res_out.push_back(est_para.dh1.a[i]);
                res_out.push_back(est_para.dh1.d[i]);
                res_out.push_back(est_para.dh1.theta[i]);
                res_out.push_back(est_para.dh1.beta[i]);
            }
            for(unsigned int i = 0; i < dof; i++)
            {
                res_out.push_back(est_para.dh2.alpha[i]);
                res_out.push_back(est_para.dh2.a[i]);
                res_out.push_back(est_para.dh2.d[i]);
                res_out.push_back(est_para.dh2.theta[i]);
                res_out.push_back(est_para.dh2.beta[i]);
            }

            for(unsigned int i = 0; i < est_para.tcp.size(); i++)
            {
                res_out.push_back(est_para.tcp[i]);
            }

            CHECK_ARRAY_CLOSE(res_ref.data(), res_out.data(),  res_ref.size(), TEST_PREC);
        }
    }
}

4. 注意事项

1. 标定参数设置
   • nom_para.base 和 nom_para.tcp 参数 和双臂标定的工装相关；
   • nom_para.choose_vec 和机械臂构型相关